#include <rclcpp/rclcpp.hpp>
#include <aim_interfaces/msg/target.hpp>
#include <aim_interfaces/msg/tracker_info.hpp>
#include <iomanip>
#include "ekf_visualizer/plotter.hpp"
#include <nlohmann/json.hpp>

using aim_interfaces::msg::Target;
using aim_interfaces::msg::TrackerInfo;

class EKFVisualizerNode : public rclcpp::Node
{
public:
    EKFVisualizerNode() : Node("ekf_visualizer_node")
    {
        // 1. Plotter参数配置与初始化
        this->declare_parameter<std::string>("plotter_host", "127.0.0.1");
        this->declare_parameter<uint16_t>("plotter_port", 9870);
        
        std::string host = this->get_parameter("plotter_host").as_string();
        uint16_t port = static_cast<uint16_t>(this->get_parameter("plotter_port").as_int());
        plotter_ = std::make_unique<tools::Plotter>(host, port);
        RCLCPP_INFO(this->get_logger(), "Plotter initialized: %s:%d", host.c_str(), port);

        // 2. 订阅 /tracker/target
        target_sub_ = this->create_subscription<Target>(
            "/tracker/target", 
            rclcpp::SensorDataQoS(),
            std::bind(&EKFVisualizerNode::targetCallback, this, std::placeholders::_1)
        );

        // 3. 订阅 /tracker/info（EKF调试数据消息）
        tracker_info_sub_ = this->create_subscription<TrackerInfo>(
            "/tracker/info",  
            rclcpp::SensorDataQoS(),
            std::bind(&EKFVisualizerNode::trackerInfoCallback, this, std::placeholders::_1)
        );

        RCLCPP_INFO(this->get_logger(), "EKF Visualizer started. Subscribed to /tracker/target and /tracker/info");
    }

private:
       void targetCallback(const Target::ConstSharedPtr& target_msg)
    {
        nlohmann::json plot_json;
        plot_json["data_type"] = "target_state";  // 标记数据类型，区分调试数据
        plot_json["tracking"] = target_msg->tracking;

        if (target_msg->tracking)
        {
            // 位置信息（EKF滤波后）
            plot_json["position"] = {
                {"x", target_msg->position.x},
                {"y", target_msg->position.y},
                {"z", target_msg->position.z}
            };
            // 速度信息（EKF滤波后）
            plot_json["velocity"] = {
                {"vx", target_msg->velocity.x},
                {"vy", target_msg->velocity.y},
                {"vz", target_msg->velocity.z}
            };
            // 偏航角与角速度（EKF滤波后）
            plot_json["yaw"] = (target_msg->yaw)*180.0/M_PI;
            plot_json["yaw_velocity"] = target_msg->v_yaw;
        }

        plotter_->plot(plot_json);
    }

    void trackerInfoCallback(const TrackerInfo::ConstSharedPtr& info_msg)
    {
        nlohmann::json plot_json;
        plot_json["data_type"] = "ekf_debug";  // 标记数据类型，明确是调试数据
        // EKF偏差数据（预测值与测量值的差值）
        plot_json["position_diff"] = info_msg->position_diff;  // 位置偏差（米）
        plot_json["yaw_diff"] = info_msg->yaw_diff;            // 偏航角偏差（弧度）
        // 未滤波原始测量数据（EKF的输入，未经过滤波）
        plot_json["unfiltered_position"] = {
            {"x", info_msg->position.x},
            {"y", info_msg->position.y},
            {"z", info_msg->position.z}
        };
        plot_json["unfiltered_yaw"] = (info_msg->yaw)*180.0/M_PI;  // 未滤波偏航角（弧度）

        plotter_->plot(plot_json);
    }

    // ------------------- 成员变量 -------------------
    // Target消息订阅者
    rclcpp::Subscription<Target>::SharedPtr target_sub_;
    // rackerInfo消息订阅者
    rclcpp::Subscription<TrackerInfo>::SharedPtr tracker_info_sub_;
    // Plotter实例（UDP发送JSON）
    std::unique_ptr<tools::Plotter> plotter_; 
};


int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<EKFVisualizerNode>();
    rclcpp::spin(node);  
    rclcpp::shutdown();
    return 0;
}